classdef LSysInfo < handle
    % Contains information about a system with a trajectory and a path.
    % Also contains member methods to calculate control.
    
    properties
        t;
        x0;
        phip;

        A;
        B;
        C;
        D;
        
        K;
    end
    
    methods
        function this = LSysInfo()
            st = load('traj_ss_lsys.mat');
            this.t = st.t;
            this.x0 = st.x0;
            this.phip = st.phip;
            
            this.A = st.A;
            this.B = st.B;
            this.C = st.C;
            this.D = st.D;
            
            this.K = st.K;
        end
        
        function [A, B, C, D] = getLinearizedModel(this, t, x)
            k = this.getNearestModel(t, x);
            A = this.A(:, :, k);
            B = this.B(:, :, k);
            C = this.C(:, :, k);
            D = this.D(:, :, k);
        end
        
        function u = getControl(this, t, x)
            k = min(length(this.t)-1, this.getNearestModel(t, x));
            u = this.K(:, :, k) * (x - this.x0(:, k));
            %u = 30*[eye(5), eye(5)] * (x - this.x0(:, k));
        end
        
        function xd = getDesiredState(this, t)
            k = this.getNearestModel(t);
            xd = this.x0(:, k);
        end
    end
    
    methods (Access = private)
        function k = getNearestModel(this, t, x)
            %k = find(abs(this.t - t) == min(abs(this.t - t)));
            jp = jointpos(x);
            p = jp(1,3);
            k = find(abs(this.phip - p) == min(abs(this.phip - p)));
        end
    end
end

